 %
 % Calculate geodesic distance (in m) between two points specified by 
 % latitude/longitude using Vincenty inverse formula for ellipsoids.
 %
 %	dist = Vincenty(p1, p2, deg2rad)
 %
 %	p1 and p2 are structs containing
 %		lat - latitude in radians
 %		lon - longitude in radians
 %
 %  deg2rad may be used to supply in degrees
 %
 % Cory Dixon
 % RECUV
 % 8-23-05
 %
function dist = Vincenty(p1, p2, deg2rad) 

if(nargin < 3)
    deg2rad = 0;
end

a = 6378137; 
b = 6356752.3142;  
f = 1/298.257223563;
lambdaP = 2*pi;

if(deg2rad)
    p1.lat = p1.lat * pi/180;
    p1.lon = p1.lon * pi/180;
    p2.lat = p2.lat * pi/180;
    p2.lon = p2.lon * pi/180;
end

p1Ind = -1;
if(length(p1.lat) == 1 )
    p1Ind = 1;
end
for i=1:length(p2.lat);

    if( p1Ind == 1 )
        L = p2.lon(i) - p1.lon(1);
        U1 = atan((1-f) * tan(p1.lat(1)));
    else
        L = p2.lon(i) - p1.lon(i);
        U1 = atan((1-f) * tan(p1.lat(i)));
    end
	U2 = atan((1-f) * tan(p2.lat(i)));
	sinU1 = sin(U1); cosU1 = cos(U1);
	sinU2 = sin(U2); cosU2 = cos(U2);
	lambda = L; 

	iterLimit = 20;
	while (abs(lambda-lambdaP) > 1e-12 && iterLimit>0) 
		iterLimit = iterLimit - 1;
		sinLambda = sin(lambda); cosLambda = cos(lambda);
		sinSigma = sqrt((cosU2*sinLambda) * (cosU2*sinLambda) + ...
			(cosU1*sinU2-sinU1*cosU2*cosLambda) * (cosU1*sinU2-sinU1*cosU2*cosLambda));
		cosSigma = sinU1*sinU2 + cosU1*cosU2*cosLambda;
		sigma = atan2(sinSigma, cosSigma);
		alpha = asin(cosU1 * cosU2 * sinLambda / sinSigma);
		cosSqAlpha = cos(alpha) * cos(alpha);
		cos2SigmaM = cosSigma - 2*sinU1*sinU2/cosSqAlpha;
		C = f/16*cosSqAlpha*(4+f*(4-3*cosSqAlpha));
		lambdaP = lambda;
		lambda = L + (1-C) * f * sin(alpha) * ...
			 (sigma + C*sinSigma*(cos2SigmaM+C*cosSigma*(-1+2*cos2SigmaM*cos2SigmaM)));
	end

	if (iterLimit==0)
		dist(i) = NaN;
        warning('Max iterations reached')
    else
        uSq = cosSqAlpha*(a*a-b*b)/(b*b);
        A = 1 + uSq/16384*(4096+uSq*(-768+uSq*(320-175*uSq)));
        B = uSq/1024 * (256+uSq*(-128+uSq*(74-47*uSq)));
        deltaSigma = B*sinSigma*(cos2SigmaM+B/4*(cosSigma*(-1+2*cos2SigmaM*cos2SigmaM)- ...
                B/6*cos2SigmaM*(-3+4*sinSigma*sinSigma)*(-3+4*cos2SigmaM*cos2SigmaM)));

        dist(i) = b*A*(sigma-deltaSigma);
    end; % formula failed to converge
end
